/*
  AeroQuad v2.4.2 - June 2011
  www.AeroQuad.com
  Copyright (c) 2011 Ted Carancho.  All rights reserved.
  An Open Source Arduino based multicopter.

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

class  Receiver {
public:
  int receiverData[LASTCHANNEL];
  int transmitterCommand[LASTCHANNEL];
  int transmitterCommandSmooth[LASTCHANNEL];
  int transmitterZero[3];
  int transmitterTrim[3];
  
  byte switchData;
  // Controls the strength of the commands sent from the transmitter
  // xmitFactor ranges from 0.01 - 1.0 (0.01 = weakest, 1.0 - strongest)
  float xmitFactor; // Read in from EEPROM
  float transmitterSmooth[LASTCHANNEL];
  float mTransmitter[LASTCHANNEL];
  float bTransmitter[LASTCHANNEL];
  //unsigned long currentTime, previousTime;

  Receiver(void) {
    transmitterCommand[ROLL] = 1500;
    transmitterCommand[PITCH] = 1500;
    transmitterCommand[YAW] = 1500;
    transmitterCommand[THROTTLE] = 1000;
    transmitterCommand[MODE] = 1000;
    transmitterCommand[AUX] = 1000;

    for (byte channel = ROLL; channel < LASTCHANNEL; channel++)
      transmitterCommandSmooth[channel] = 1.0;
    for (byte channel = ROLL; channel < THROTTLE; channel++)
      transmitterZero[channel] = 1500;
  }

  // ******************************************************************
  // The following function calls must be defined in any new subclasses
  // ******************************************************************
  virtual void initialize(void);
  virtual void read(void);

  // **************************************************************
  // The following functions are common between all Gyro subclasses
  // **************************************************************

  void _initialize(void) {
    xmitFactor = readFloat(XMITFACTOR_ADR);
    
    mTransmitter[0] = readFloat(RECEIVER_CHANNEL_0_SLOPE_ADR);
    bTransmitter[0] = readFloat(RECEIVER_CHANNEL_0_OFFSET_ADR);
    transmitterSmooth[0] = readFloat(RECEIVER_CHANNEL_0_SMOOTH_FACTOR_ADR);
    mTransmitter[1] = readFloat(RECEIVER_CHANNEL_1_SLOPE_ADR);
    bTransmitter[1] = readFloat(RECEIVER_CHANNEL_1_OFFSET_ADR);
    transmitterSmooth[1] = readFloat(RECEIVER_CHANNEL_1_SMOOTH_FACTOR_ADR);
    mTransmitter[2] = readFloat(RECEIVER_CHANNEL_2_SLOPE_ADR);
    bTransmitter[2] = readFloat(RECEIVER_CHANNEL_2_OFFSET_ADR);
    transmitterSmooth[2] = readFloat(RECEIVER_CHANNEL_2_SMOOTH_FACTOR_ADR);
    mTransmitter[3] = readFloat(RECEIVER_CHANNEL_3_SLOPE_ADR);
    bTransmitter[3] = readFloat(RECEIVER_CHANNEL_3_OFFSET_ADR);
    transmitterSmooth[3] = readFloat(RECEIVER_CHANNEL_3_SMOOTH_FACTOR_ADR);
    mTransmitter[4] = readFloat(RECEIVER_CHANNEL_4_SLOPE_ADR);
    bTransmitter[4] = readFloat(RECEIVER_CHANNEL_4_OFFSET_ADR);
    transmitterSmooth[4] = readFloat(RECEIVER_CHANNEL_4_SMOOTH_FACTOR_ADR);
    mTransmitter[5] = readFloat(RECEIVER_CHANNEL_5_SLOPE_ADR);
    bTransmitter[5] = readFloat(RECEIVER_CHANNEL_5_OFFSET_ADR);
    transmitterSmooth[5] = readFloat(RECEIVER_CHANNEL_5_SMOOTH_FACTOR_ADR);
  }
  
  const boolean getMotorArmed(){
  	return (switchData >> 2) & 0x01;
  }
  
  const boolean getHoverOn(){
  	return (switchData >> 0) & 0x01;
  }

  // returns non-smoothed non-scaled ADC data in PWM full range 1000-2000 values
  const int getRaw(byte channel) {
    return receiverData[channel];
  }
  
  // returns raw but smoothed receiver(channel) in PWM
  const int getRawSmoothed(byte channel) {
    return transmitterCommandSmooth[channel];
  }
 
   // returns smoothed & scaled receiver(channel) in PWM values, zero centered
  const int getData(byte channel) {
    return transmitterCommand[channel];
  }
  
  // return the smoothed & scaled number of radians/sec in stick movement - zero centered
  const float getSIData(byte channel) {
    // 2.3 Original
    return ((transmitterCommand[channel] - transmitterZero[channel]) * (2.5 * PWM2RAD));  // +/- 2.5RPS 50% of full rate
    // 2.3 Stable
    //return ((transmitterCommand[channel] - transmitterZero[channel]) * (5.0 * PWM2RAD));  // +/- 5RPS factored by xmitfactor of full rate
  }

  const int getTrimData(byte channel) {
    return receiverData[channel] - transmitterTrim[channel];
  }

  // returns Zero value of channel in PWM
  const int getZero(byte channel) {
    return transmitterZero[channel];
  }
  // sets zero value of channel in PWM
  void setZero(byte channel, int value) {
    transmitterZero[channel] = value;
  }

  const int getTransmitterTrim(byte channel) {
    return transmitterTrim[channel];
  }

  void setTransmitterTrim(byte channel, int value) {
    transmitterTrim[channel] = value;
  }

  const float getSmoothFactor(byte channel) {
    return transmitterSmooth[channel];
  }

  void setSmoothFactor(byte channel, float value) {
    transmitterSmooth[channel] = value;
  }

  const float getXmitFactor(void) {
    return xmitFactor;
  }

  void setXmitFactor(float value) {
    xmitFactor = value;
  }

  const float getTransmitterSlope(byte channel) {
    return mTransmitter[channel];
  }

  void setTransmitterSlope(byte channel, float value) {
    mTransmitter[channel] = value;
  }

  const float getTransmitterOffset(byte channel) {
    return bTransmitter[channel];
  }

  void setTransmitterOffset(byte channel, float value) {
    bTransmitter[channel] = value;
  }

  const float getAngle(byte channel) {
    // Scale 1000-2000 usecs to -45 to 45 degrees
    // m = 0.09, b = -135
    return (0.09 * transmitterCommand[channel]) - 135;
    //return (0.09 * receiverData[channel]) - 135;
  }
};

/*************************************************/
/*************** AeroQuad PCINT ******************/
/*************************************************/
#if defined(AeroQuad_v1) || defined(AeroQuad_v18) || defined(AeroQuad_Mini) || defined(AeroQuad_Wii) || defined(AeroQuad_v1_IDG)
/*
volatile uint8_t *port_to_pcmask[] = {
  &PCMSK0,
  &PCMSK1,
  &PCMSK2
};
volatile static uint8_t PCintLast[3];
// Channel data
typedef struct {
  byte edge;
  unsigned long riseTime;
  unsigned long fallTime;
  unsigned int  lastGoodWidth;
} tPinTimingData;
volatile static tPinTimingData pinData[9];

// Attaches PCINT to Arduino Pin
void attachPinChangeInterrupt(uint8_t pin) {
  uint8_t bit = digitalPinToBitMask(pin);
  uint8_t port = digitalPinToPort(pin);
  volatile uint8_t *pcmask;

  // map pin to PCIR register
  if (port == NOT_A_PORT) {
    return;
  }
  else {
    port -= 2;
    pcmask = port_to_pcmask[port];
  }
  // set the mask
  *pcmask |= bit;
  // enable the interrupt
  PCICR |= 0x01 << port;
}

// ISR which records time of rising or falling edge of signal
static void measurePulseWidthISR(uint8_t port, uint8_t pinoffset) {
  uint8_t bit;
  uint8_t curr;
  uint8_t mask;
  uint8_t pin;
  uint32_t currentTime;
  uint32_t time;

  // get the pin states for the indicated port.
  curr = *portInputRegister(port+2);
  mask = curr ^ PCintLast[port];
  PCintLast[port] = curr;
  // mask is pins that have changed. screen out non pcint pins.
  if ((mask &= *port_to_pcmask[port]) == 0) {
    return;
  }
  currentTime = micros();
  // mask is pcint pins that have changed.
  for (uint8_t i=0; i < 8; i++) {
    bit = 0x01 << i;
    if (bit & mask) {
      pin = pinoffset + i;
      // for each pin changed, record time of change
      if (bit & PCintLast[port]) {
        time = currentTime - pinData[pin].fallTime;
        pinData[pin].riseTime = currentTime;
        if ((time >= MINOFFWIDTH) && (time <= MAXOFFWIDTH))
          pinData[pin].edge = RISING_EDGE;
        else
          pinData[pin].edge = FALLING_EDGE; // invalid rising edge detected
      }
      else {
        time = currentTime - pinData[pin].riseTime;
        pinData[pin].fallTime = currentTime;
        if ((time >= MINONWIDTH) && (time <= MAXONWIDTH) && (pinData[pin].edge == RISING_EDGE)) {
          pinData[pin].lastGoodWidth = time;
          pinData[pin].edge = FALLING_EDGE;
        }
      }
    }
  }
}

*/
class Receiver_AeroQuad : public Receiver {
public:  
  // Configure each receiver pin for PCINT
  void initialize() {
    this->_initialize(); // load in calibration xmitFactor from EEPROM
    
    // Check if i2c resiever is connected
    /*
    if (readWhoI2C(accelAddress) != 0x02) // page 52 of datasheet
      Serial.println("I2C Resiever not found!");
      
    */
    /*
    for (byte channel = ROLL; channel < LASTCHANNEL; channel++) {
      pinMode(receiverPin[channel], INPUT);
      pinData[receiverPin[channel]].edge = FALLING_EDGE;
      attachPinChangeInterrupt(receiverPin[channel]);
    }
    */
  }

  void read(void) {
    //I2C Address of the Resiever
    int deviceAddress = 2; 
    
    Wire.requestFrom(deviceAddress, 13);

    for(byte channel = ROLL; channel < LASTCHANNEL; channel++) {

      uint16_t length = ((Wire.receive()|(Wire.receive() << 8)));

      receiverData[channel] = (mTransmitter[channel] * length) + bTransmitter[channel];
      transmitterCommandSmooth[channel] = filterSmooth(receiverData[channel], transmitterCommandSmooth[channel], transmitterSmooth[channel]);
    }
    
    switchData = Wire.receive();
    


      /*
    for(byte channel = ROLL; channel < LASTCHANNEL; channel++) {
      byte pin = receiverPin[channel];
      uint8_t oldSREG = SREG;
      cli();
      // Get receiver value read by pin change interrupt handler
      uint16_t lastGoodWidth = pinData[pin].lastGoodWidth;
      SREG = oldSREG;

      // Apply transmitter calibration adjustment
      receiverData[channel] = (mTransmitter[channel] * lastGoodWidth) + bTransmitter[channel];
      // Smooth the flight control transmitter inputs
      transmitterCommandSmooth[channel] = filterSmooth(receiverData[channel], transmitterCommandSmooth[channel], transmitterSmooth[channel]);
    }
    */

    // Reduce transmitter commands using xmitFactor and center around 1500
    for (byte channel = ROLL; channel < LASTCHANNEL; channel++)
      if (channel < THROTTLE)
        transmitterCommand[channel] = ((transmitterCommandSmooth[channel] - transmitterZero[channel]) * xmitFactor) + transmitterZero[channel];
      else
        // No xmitFactor reduction applied for throttle, mode and AUX
        transmitterCommand[channel] = transmitterCommandSmooth[channel];
    //transmitterCommand[THROTTLE] = (transmitterCommand[THROTTLE] * 0.8) + 200; // lower max throttle to allow for motor control
    
  }
  
};
#endif

